//
// Created by augtons on 2022/1/18.
//

#ifndef ESP32IMU_IMU_H
#define ESP32IMU_IMU_H

#include "math.h"
#include "stdint.h"
#include "esp_timer.h"
#include "esp_log.h"

typedef struct{
    float q0;
    float q1;
    float q2;
    float q3;
}Quaternion;

typedef struct {
    float x;
    float y;
    float z;
}Vector3;

typedef struct {
    uint64_t lastTime;
    Quaternion quaternion;
    Vector3 error_Int;
    float kp;
    float ki;
}Imu;

typedef struct {
    float yaw;  // 偏航角
    float pitch;// 俯仰角
    float roll;// 横滚角
}EulerAngle;

static float invSqrt(float x);

/**
 * @brief 更新四元数
 * @param[in] imu   IMU对象
 * @param[in] accel 加速度
 * @param[in] gyro 角速度
 */
void IMU_Update(Imu *imu, Vector3 *accel, Vector3 *gyro);

/**
 * @brief 根据四元数求欧拉角
 * @param[in] quaternion
 * @param[out] eulerAngle
 */
void IMU_GetEulerAngle(Quaternion *quaternion, EulerAngle *eulerAngle);

/**
 * @brief 根据四元数求传感器法向量倾角和方位角
 * @param[in] quaternion
 * @param[out] tiltAngle 倾角
 * @param[out] azimuth 方位角
 */
void IMU_GetTiltAngle(Quaternion *quaternion, float *tiltAngle, float *azimuth);

#endif //ESP32IMU_IMU_H
